#include "track.h"

#include <pthread.h>
#include <unistd.h>

#include <algorithm>
#include <atomic>
#include <cmath>
#include <filesystem>
#include <functional>
#include <mutex>
#include <random>
#include <thread>
#include <unordered_map>

#include "dwarf_utils.h"
#include "elog.h"
#include "elog_file.h"
#include "motor_controller.h"
#include "rkmedia_api.h"
#include "siamRpnTracker.h"
#include "ws_response.h"

using namespace std;
namespace fs = std::filesystem;

static std::mutex tracker_state_mutex;
static std::atomic<bool> b_tracker_init = false;
static std::atomic<bool> b_tracking = false;
static std::atomic<StateNotify> tracker_state = STATE_IDLE;
static std::atomic<StateSentryModeNotify> sentry_mode_state =
    STATE_SENTRY_MODE_IDLE;

static StepMotorUser &motor_yaw = MotorController::motorYaw();
static StepMotorUser &motor_pitch = MotorController::motorPitch();
static SiamRpnTracker *siam_rpn_tracker;
static YoloV8 *g_yolov8;
// cv::Ptr<cv::ml::RTrees> g_obj_move_model;
static int g_pid_min_speed = 15;
static bool sentry_mode_ = false;
static std::mutex g_track_pid_mutex;
static TrackState g_track_state;
static track_result g_track_result;
static std::vector<OneEuroFilter *> g_track_result_filter;
static std::thread g_pid_thread;

// static int final_speed_max = 4000;
void pid_thread(Track *track)
{
    int track_start_limit_speed = 4000;
    int trackt_limit_speed_max = 4000;
    char pid_x_mode = 0;
    char pid_x_mode_old = 0;
    char pid_y_mode = 0;
    char pid_y_mode_old = 0;
    log_i("pid_thread in start.");

    int last_motor_speed_x = 0, last_motor_speed_y = 0;
    int motor_speed_x = 0, motor_speed_y = 0;
    int last_dir_x = 0, last_dir_y = 0;

    float pid_tau = 0.02f;
    float pid_lim_min = -20000.0f;
    float pid_lim_max = 20000.f;
    float pid_lim_min_int = -5000.0f;
    float pid_lim_max_int = 5000.0f;
    float sample_time_s = 0.1f;

    float pid_x_kp = 11.0f; // float pid_x_kp = 12.1f;
    float pid_x_ki = 0.6f;
    float pid_x_kd = 0.2f;

    float pid_y_kp = 9.0f;
    float pid_y_ki = 0.4f;
    float pid_y_kd = 0.2f;
    PIDController pid_x = {
        pid_x_kp, pid_x_ki, pid_x_kd, pid_tau,
        pid_lim_min, pid_lim_max, pid_lim_min_int, pid_lim_max_int,
        sample_time_s};

    PIDController pid_y = {
        pid_y_kp, pid_y_ki, pid_y_kd, pid_tau,
        pid_lim_min, pid_lim_max, pid_lim_min_int, pid_lim_max_int,
        sample_time_s};

    TrackPID track_pid_x, track_pid_y;
    track_pid_x.init(&pid_x);
    track_pid_y.init(&pid_y);

    cv::Size frame_size(1280, 720);
    Point2d center_pos =
        Point2d((frame_size.width - 1) / 2, (frame_size.height - 1) / 2);

    float measurement_x = 0.f, measurement_y = 0.f;
    float last_measurement_x = 0.f, last_measurement_y = 0.f;
    float accum_delta_x = 0.f, accum_delta_y = 0.f;
    int calc_inter_time = 5;
    int lost_num = 0;

    std::vector<cv::Point2d> obj_pt_arr;
    std::vector<int> step_relative_pos;
    std::vector<float> measurement_x_arr;
    std::vector<float> measurement_y_arr;
    while (!g_track_state.track_finish_)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(calc_inter_time));

        if (g_track_state.track_redetect_)
        {
            log_i("g_track_state.track_redetect_ : %d",
                  g_track_state.track_redetect_);
            motor_yaw.driver.stop(1, 0, 0);
            motor_pitch.driver.stop(1, 0, 0);
            // motor_speed_x *= 0.9;
            // motor_speed_y *= 0.9;
            std::this_thread::sleep_for(std::chrono::milliseconds(20));
            continue;
        }

        if (g_track_state.track_param_update_)
        {
            log_i("### pid track 0. track_param_update_=%d, measurement_x=%f, last_measurement_x = % f, accum_delta_x = % f, bscore = % f ",
                  g_track_state.track_param_update_,
                  measurement_x,
                  last_measurement_x, accum_delta_x, g_track_result.bscore);
            if (g_track_state.track_lost_)
            {
                std::unique_lock<std::mutex> locker(g_track_pid_mutex);
                g_track_state.track_param_update_ = false;
                locker.unlock();
                continue;
            }

            std::unique_lock<std::mutex> locker(g_track_pid_mutex);
            g_track_state.track_param_update_ = false;
            locker.unlock();

            measurement_x = g_track_result.center_pos.x - center_pos.x;
            measurement_y = g_track_result.center_pos.y - center_pos.y;

            log_i("####### measurement_x=%f, measurement_y=%f", measurement_x,
                  measurement_y);
            measurement_x_arr.push_back(measurement_x);
            if (measurement_x_arr.size() > 15)
                measurement_y_arr.push_back(measurement_y);
            if (measurement_y_arr.size() > 15)
            {
                measurement_y_arr.erase(measurement_y_arr.begin());
            }
            // float k_thre_x = 3.5;
            // float kp_ratio_x =
            //     track->pidKpRatio(measurement_x_arr, frame_size.width, k_thre_x);
            // pid_x.Kp = (pid_x_kp)*kp_ratio_x;

            // float k_thre_y = 3.5 / (1.0f * frame_size.width / frame_size.height);
            // float kp_ratio_y =
            //     track->pidKpRatio(measurement_y_arr, frame_size.height, k_thre_y);
            // pid_y.Kp = pid_y_kp * kp_ratio_y;
            // if (abs(measurement_x) < 50.0f && abs(measurement_x) > 10.0f) // frame_size(1280, 720);
            if (abs(measurement_x) < 20.0f)
            {
                pid_x.Kp = (pid_x_kp)*abs(measurement_x) / 40.0f;
                pid_x_mode = 0;
            }
            else if (abs(measurement_x) >= 10.0f)
            {
                float k_thre_x = 3.5;
                float kp_ratio_x =
                    track->pidKpRatio(measurement_x_arr, frame_size.width, k_thre_x);
                pid_x.Kp = (pid_x_kp)*kp_ratio_x;
                pid_x_mode = 1;
            }
            // else
            // {
            //     // pid_x.Kd = pid_x_kd / 4;
            //     pid_x.Kp = pid_x_kp / 5;
            //     pid_x_mode = 2;
            // }
            // if (pid_x_mode != pid_x_mode_old)
            // {
            //     if (pid_x_mode == 0)
            //     {
            //         pid_x.Ki = pid_x_ki * 2.0f;
            //         if (pid_x_mode_old == 1)
            //         {
            //             pid_x.integrator /= 2;
            //         }
            //         else
            //         {
            //             pid_x.integrator /= 2;
            //         }
            //     }
            //     else if (pid_x_mode == 1)
            //     {
            //         pid_x.Ki = pid_x_ki;
            //     }
            //     else if (pid_x_mode == 2)
            //     {
            //         pid_x.Ki = pid_x_ki * 3.0f;
            //         // pid_x.integrator /= 2;
            //     }
            // }
            // pid_x_mode_old = pid_x_mode;

            // // if (abs(measurement_y) < 50.0f && (measurement_y) > 40.0f) // frame_size(1280, 720);
            if (abs(measurement_y) < 10.0f)
            {
                pid_y.Kp = (pid_y_kp)*abs(measurement_y) / 20.0f;
                pid_y_mode = 0;
            }
            else if (abs(measurement_y) >= 10.0f)
            {
                pid_y_mode = 1;
                float k_thre_y = 3.5 / (1.0f * frame_size.width / frame_size.height);
                float kp_ratio_y =
                    track->pidKpRatio(measurement_y_arr, frame_size.height, k_thre_y);
                pid_y.Kp = pid_y_kp * kp_ratio_y;
            }
            // else
            // {
            //     pid_y.Kp = pid_y_kp / 5.0f;
            //     pid_y_mode = 2;
            // }

            // if (pid_y_mode != pid_y_mode_old)
            // {
            //     if (pid_y_mode == 0)
            //     {
            //         pid_y.Ki = pid_y_ki * 2.0f;
            //         if (pid_y_mode_old == 1)
            //         {
            //             pid_y.integrator /= 2;
            //         }
            //         else
            //         {
            //             pid_y.integrator /= 2;
            //         }
            //     }
            //     else if (pid_y_mode == 1)
            //     {
            //         pid_y.Ki = pid_y_ki;
            //     }
            //     else if (pid_y_mode == 2)
            //     {
            //         pid_y.Ki = pid_y_ki * 3.0f;
            //         // pid_y.integrator /= 2;
            //     }
            // }
            // pid_y_mode_old = pid_y_mode;

            if (abs(measurement_x) > 5.0f)
            {
                track_pid_x.update(&pid_x, 0.f, measurement_x);
            }
            else
            {
                // pid_x.integrator /= 2;
                motor_yaw.driver.stop(1, 0, 0);
                track_pid_x.init(&pid_x);
            }
            if (abs(measurement_y) > 3.0f)
            {
                track_pid_y.update(&pid_y, 0.f, measurement_y);
            }
            else
            {
                // pid_y.integrator /= 2;
                motor_pitch.driver.stop(1, 0, 0);
                track_pid_y.init(&pid_y);
            }

            motor_speed_x = pid_x.out;
            motor_speed_y = pid_y.out;

            accum_delta_x = 0.f;
            accum_delta_y = 0.f;
        }
        else
        {
            if (!g_track_state.track_object_moving_ || g_track_state.track_lost_)
            {
                float delta_x = track->convertSpeed2Pixel(
                    3.189f, motor_speed_x / 32, calc_inter_time, frame_size.width);
                float delta_y = track->convertSpeed2Pixel(
                    1.794f, motor_speed_y / 32, calc_inter_time, frame_size.height);
                measurement_x = last_measurement_x + delta_x;
                measurement_y = last_measurement_y + delta_y;

                accum_delta_x += delta_x;
                accum_delta_y += delta_y;

                track_pid_x.update(&pid_x, 0.f, measurement_x);
                track_pid_y.update(&pid_y, 0.f, measurement_y);

                motor_speed_x = pid_x.out;
                motor_speed_y = pid_y.out;
                log_i("### pid track 1.  motor_speed_x=%d, motor_speed_y=%d",
                      motor_speed_x, motor_speed_y);
            }
            else
            {
                continue;
            }
        }

        if (g_track_state.track_first_start)
        {
            track_pid_x.init(&pid_x);
            track_pid_y.init(&pid_y);
            motor_speed_x = motor_speed_x > 0 ? std::min(motor_speed_x, track_start_limit_speed) : std::max(motor_speed_x, -track_start_limit_speed);
            motor_speed_y = motor_speed_y > 0 ? std::min(motor_speed_y, track_start_limit_speed / 2) : std::max(motor_speed_y, -track_start_limit_speed / 2);
        }
        int trend_x = (abs(last_motor_speed_x) < abs(motor_speed_x)) ? 1 : 0;
        int trend_y = (abs(last_motor_speed_y) < abs(motor_speed_y)) ? 1 : 0;

        int dir_x = (motor_speed_x > 0) ? 0 : 1;
        int dir_y = (motor_speed_y > 0) ? 1 : 0;

        log_i("### pid track 2. motor_speed_x=%d, trend_x=%d, dir_x=%d, motor_speed_y=%d, trend_y=%d, dir_y=%d",
              motor_speed_x, trend_x, dir_x, motor_speed_y, trend_y, dir_y);

        if (abs(motor_speed_x) < g_pid_min_speed)
        {
            motor_yaw.driver.stop(1, 0, 0);
        }
        else
        {
            motor_yaw.driver.checkMotionState();
            std::this_thread::sleep_for(std::chrono::milliseconds(2));
            if (motor_yaw.driver.getMotionState() == STEP_MOTOR_STOPPED)
                motor_yaw.driver.run(1, STEP_MOTOR_MINISTEP_32, dir_x,
                                     abs(motor_speed_x), 0, 0);
            else
            {
                motor_yaw.driver.setFrequency(trend_x, abs(motor_speed_x), 0);
                motor_yaw.driver.setDirection(dir_x);
            }
        }

        if (abs(motor_speed_y) < g_pid_min_speed)
        {
            motor_pitch.driver.stop(1, 0, 0);
        }
        else
        {
            motor_pitch.driver.checkMotionState();
            std::this_thread::sleep_for(std::chrono::milliseconds(2));
            if (motor_pitch.driver.getMotionState() == STEP_MOTOR_STOPPED)
                motor_pitch.driver.run(1, STEP_MOTOR_MINISTEP_32, dir_y,
                                       abs(motor_speed_y), 0, 0);
            else
            {
                motor_pitch.driver.setFrequency(trend_y, abs(motor_speed_y), 0);
                motor_pitch.driver.setDirection(dir_y);
            }
        }

        last_motor_speed_x = motor_speed_x;
        last_motor_speed_y = motor_speed_y;
        last_measurement_x = measurement_x;
        last_measurement_y = measurement_y;
        std::ofstream motor_speed_x_out_file("motor_speed_x_out_file.txt", std::ios::out | std::ios::app);
        motor_speed_x_out_file << motor_speed_x << std::endl;
        motor_speed_x_out_file.close();

        std::ofstream measurement_x_file("measurement_x.txt", std::ios::out | std::ios::app);
        measurement_x_file << measurement_x << std::endl;
        measurement_x_file.close();

        std::ofstream motor_speed_y_out_file("motor_speed_y_out_file.txt", std::ios::out | std::ios::app);
        motor_speed_y_out_file << motor_speed_y << std::endl;
        motor_speed_y_out_file.close();

        std::ofstream measurement_y_file("measurement_y.txt", std::ios::out | std::ios::app);
        measurement_y_file << measurement_y << std::endl;
        measurement_y_file.close();
    }

    log_i("pid_thread in end.");
    motor_yaw.driver.stop(1, 0, 0);
    motor_pitch.driver.stop(1, 0, 0);
}

Track::Track()
{
    cam_tele_ = CamTele();
    cam_wide_ = CamWide();
    focus_ = Focus();
}

Track::~Track()
{
}

static unordered_map<int, int (Track::*)(const WsPacket &ws_packet)>
    track_func_table{{CMD_SENTRY_MODE_START, &Track::startTrackSentryMode},
                     {CMD_SENTRY_MODE_STOP, &Track::stopTrack},
                     {CMD_TRACK_START_TRACK, &Track::startTrackNew},
                     {CMD_TRACK_STOP_TRACK, &Track::stopTrack}};

int Track::handleMessage(const WsPacket &ws_packet)
{
    log_i("handle track message, cmd = %d", ws_packet.cmd());

    // �ڱ��в��Һ���ָ�벢����
    auto it = track_func_table.find(ws_packet.cmd());
    if (it != track_func_table.end())
    {
        (this->*(it->second))(ws_packet);
    }
    else
    {
        log_i("Function not found");
    }

    return 0;
}

int Track::initTrack()
{
    log_i("init track");
    if (b_tracker_init == false)
    {
        log_i("loading model");
        siam_rpn_tracker = new SiamRpnTracker();
        // g_obj_move_model =
        // cv::ml::StatModel::load<cv::ml::RTrees>("/userdata/install/baike_test/move_random_forest_model.xml");
        // // ���ģ���Ƿ�ɹ�����
        // if (g_obj_move_model.empty()) {
        //     std::cerr << "Failed to load model!" << std::endl;
        //     return -1; // ����ʧ�ܣ����ش�����
        // } else {
        //     std::cout << "Model loaded successfully." << std::endl;
        // }

        g_yolov8 = new YoloV8(
            "/system/model/"
            "best_u8_train0319_optparam_precompile.crypt");
        b_tracker_init = true;
    }

    return 0;
}

void Track::notifyTrackState()
{
    std::lock_guard<std::mutex> lock(tracker_state_mutex);
    log_i("sentry_mode_state:%d, sentry_mode_ : %d, tracker_state : %d", sentry_mode_state.load(), sentry_mode_, tracker_state.load());
    if (sentry_mode_)
    {
        if (sentry_mode_state != STATE_SENTRY_MODE_IDLE)
        {
            log_i("sentry_mode_state:%d", sentry_mode_state.load());
            wsResNotifySentryModeState(CMD_NOTIFY_SENTRY_MODE_STATE, MODULE_NOTIFY,
                                       CMD_TYPE_NOTIFICATION, sentry_mode_state);
        }
    }
    else
    {
        if (tracker_state != STATE_IDLE)
        {
            log_i("tracker_state:%d", tracker_state.load());
            wsResNotifyCamFunctionState(CMD_NOTIFY_TELE_FUNCTION_STATE, MODULE_NOTIFY,
                                        CMD_TYPE_NOTIFICATION, CAM_FUNCTION_TRACK,
                                        tracker_state);
        }
    }
}

StateNotify Track::getTrackState()
{
    if (sentry_mode_)
    {
        if (sentry_mode_state != STATE_SENTRY_MODE_IDLE)
        {
            return STATE_RUNNING;
        }
    }

    return tracker_state;
}

// ʹ���߳���������ȡ��״̬������֪ͨ��״̬֮���˳��ȷ�������״̬�����ȷ���
void Track::setAndNotifyTrackState(const StateNotify &state)
{
    std::lock_guard<std::mutex> lock(tracker_state_mutex);

    tracker_state = state;

    log_i("tracker_state:%d, sentry_mode : %d", tracker_state.load(),
          sentry_mode_);

    wsResNotifyCamFunctionState(CMD_NOTIFY_TELE_FUNCTION_STATE, MODULE_NOTIFY,
                                CMD_TYPE_NOTIFICATION, CAM_FUNCTION_TRACK, state);
}

void Track::setAndNotifySentryModeState(const StateSentryModeNotify &state)
{
    std::lock_guard<std::mutex> lock(tracker_state_mutex);

    sentry_mode_state = state;

    log_i("sentry mode state:%d, sentry_mode : %d", sentry_mode_state.load(),
          sentry_mode_);

    wsResNotifySentryModeState(CMD_NOTIFY_SENTRY_MODE_STATE, MODULE_NOTIFY,
                               CMD_TYPE_NOTIFICATION, state);
}

int Track::startTrackNew(const WsPacket &ws_packet)
{
    log_i("startTrackNew start\r\n\r\n\r\n");
    // const char* filename1 = "motor_speed_x_out_file.txt"; // ɾ���ļ�
    // if (std::remove(filename1) == 0)
    // {
    //     log_i("successfully deleted motor_speed_x_out_file\r\n\r\n\r\n");
    // }
    // const char* filename2 = "measurement_x.txt"; // ɾ���ļ�
    // if (std::remove(filename2) == 0)
    // {
    //     log_i("successfully deleted measurement_x\r\n\r\n\r\n");
    // }
    // const char* filename3 = "box_center_pos_x.txt"; // ɾ���ļ�
    // if (std::remove(filename3) == 0)
    // {
    //     log_i("successfully deleted box_center_pos_x\r\n\r\n\r\n");
    // }
    sentry_mode_ = false;
    ReqStartTrack req_start_track;
    string serialized_data = ws_packet.data();

    if (!req_start_track.ParseFromString(serialized_data))
    {
        wsCommonResponse(CMD_TRACK_START_TRACK, MODULE_TRACK, CMD_TYPE_RESPONSE,
                         -WS_PARSE_PROTOBUF_ERROR);
        return -WS_PARSE_PROTOBUF_ERROR;
    }

    if (b_tracker_init == false)
    {
        wsCommonResponse(CMD_TRACK_START_TRACK, MODULE_TRACK, CMD_TYPE_RESPONSE,
                         -CODE_TRACK_TRACKER_INITING);
        return -CODE_TRACK_TRACKER_INITING;
    }

    if (cam_tele_.isCameraOpen() < 0)
    {
        wsCommonResponse(CMD_TRACK_START_TRACK, MODULE_FOCUS, CMD_TYPE_RESPONSE,
                         -CODE_CAMERA_TELE_CLOSED);
        return -CODE_CAMERA_TELE_CLOSED;
    }

    if (tracker_state == STATE_RUNNING)
    {
        stopTrack();
        while (tracker_state != STATE_IDLE)
            usleep(50 * 1000);
    }

    setAndNotifyTrackState(STATE_RUNNING);

    rgb_power_.setRgbEffect(RGB_EFFECT_WORKING_STATE);

    cv::Size frame_size(1280, 720);
    MEDIA_BUFFER frame_mb;
    uint64_t frame_mb_id;
    cam_tele_.getNv12MediaBufferWithID(&frame_mb, &frame_mb_id);
    MEDIA_BUFFER frame_mb_copy = NULL;

    if (!frame_mb)
    {
        log_e("get frame_mb failed");
        wsCommonResponse(CMD_TRACK_START_TRACK, MODULE_TRACK, CMD_TYPE_RESPONSE,
                         -CODE_TRACK_TRACKER_FAILED);
        setAndNotifyTrackState(STATE_STOPPED);
        setAndNotifyTrackState(STATE_IDLE);

        rgb_power_.setRgbEffect(RGB_EFFECT_FAIL);

        return -CODE_TRACK_TRACKER_FAILED;
    }

    int x = req_start_track.x();
    int y = req_start_track.y();
    int w = req_start_track.w();
    int h = req_start_track.h();

    // int x = 532;
    // int y = 308;
    // int w = 180;
    // int h = 83;

    Rect2d box = Rect2d(x, y, w, h);
    Point2d center_pos =
        Point2d((frame_size.width - 1) / 2, (frame_size.height - 1) / 2);

    time_checker time1, time2;
    time1.start();
    log_i("start init tracker");

    int ret = siam_rpn_tracker->init(frame_mb, frame_size, box);
    rkfree(frame_mb);
    if (!ret)
    {
        log_e("preprocess failed");
        wsCommonResponse(CMD_TRACK_START_TRACK, MODULE_TRACK, CMD_TYPE_RESPONSE,
                         -CODE_TRACK_TRACKER_FAILED);
        setAndNotifyTrackState(STATE_STOPPED);
        setAndNotifyTrackState(STATE_IDLE);

        rgb_power_.setRgbEffect(RGB_EFFECT_FAIL);

        return -CODE_TRACK_TRACKER_FAILED;
    }
    time1.stop();

    Point2d box_center_pos =
        box.tl() + Point2d((box.width - 1) / 2, (box.height - 1) / 2);

    if (g_pid_thread.joinable())
        g_pid_thread.join();
    g_pid_thread = std::thread(&pid_thread, this);

    int lost_num = 0;
    b_tracking = true;
    g_track_result.center_pos = center_pos;
    g_track_state.track_finish_ = false;
    g_track_state.track_param_update_ = false;
    g_track_state.track_lost_ = false;
    g_track_state.track_object_moving_ = true;
    g_track_state.track_first_start = true;
    g_track_state.track_redetect_ = false;

    auto filter_x = new OneEuroFilter(30, 0.5, 0.1, 30.0);
    auto filter_y = new OneEuroFilter(30, 0.5, 0.1, 30.0);
    auto filter_w = new OneEuroFilter(30, 0.5, 0.1, 30.0);
    auto filter_h = new OneEuroFilter(30, 0.5, 0.1, 30.0);
    auto filter_center_pos_x = new OneEuroFilter(30, 0.5, 0.1, 30.0);
    auto filter_center_pos_y = new OneEuroFilter(30, 0.5, 0.1, 30.0);
    g_track_result_filter.push_back(filter_x);
    g_track_result_filter.push_back(filter_y);
    g_track_result_filter.push_back(filter_w);
    g_track_result_filter.push_back(filter_h);
    g_track_result_filter.push_back(filter_center_pos_x);
    g_track_result_filter.push_back(filter_center_pos_y);

    int track_frame_count = 0;
    while (b_tracking)
    {
        time1.start();
        // frame_mb = cam_tele_.getNv12MediaBuffer();
        uint64_t cur_frame_mb_id;
        cam_tele_.getNv12MediaBufferWithID(&frame_mb, &cur_frame_mb_id);

        if (frame_mb_id - cur_frame_mb_id == 0)
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(3));
            if (frame_mb != NULL)
            {
                rkfree(frame_mb);
            }
            continue;
        }

        frame_mb_id = cur_frame_mb_id;

        track_frame_count++;
        if (track_frame_count > first_frame_cnt)
        {
            g_track_state.track_first_start = false;
        }

        if (frame_mb != NULL)
        {
            frame_mb_copy = RK_MPI_MB_Copy(frame_mb, RK_TRUE);
            // time2.start();
            int ret = siam_rpn_tracker->preprocess_img(frame_mb, frame_size,
                                                       box_center_pos); // preprocess
            rkfree(frame_mb);
            if (ret < 0)
            {
                log_e("preprocess failed");
                rkfree(frame_mb_copy);
                break;
            }
            track_result result =
                siam_rpn_tracker->track(frame_size, g_track_state.track_redetect_);
            // time2.stop();
            // time2.show_distance("Update stage ---- track cost time");

            log_i("######## result.bscore : %f, lost_num : %d, g_track_state.track_lost_ : % d, g_track_state.track_redetect_ : % d ",
                  result.bscore,
                  lost_num, g_track_state.track_lost_,
                  g_track_state.track_redetect_);
            if (result.bscore < 0.1)
            {
                lost_num++;
                if (lost_num >= 10)
                {
                    g_track_state.track_lost_ = true;
                    g_track_state.track_redetect_ = true;
                }
                else if (lost_num >= 1)
                {
                    g_track_state.track_lost_ = true;
                }
            }

            if (g_track_state.track_redetect_)
            {
                if (result.bscore > 0.98)
                {
                    lost_num = 0;
                    g_track_state.track_redetect_ = false;
                    g_track_state.track_lost_ = false;
                }
            }

            if (lost_num > 90)
            {
                lost_num = 0;
                rkfree(frame_mb_copy);
                break;
            }

            if (g_track_state.track_redetect_ || (g_track_state.track_lost_ && result.bscore < 0.6))
            {
                if (result.bscore >= 0.1)
                {
                    lost_num++;
                }
                rkfree(frame_mb_copy);
                wsResNotifyTrackResult(CMD_NOTIFY_TRACK_RESULT, MODULE_NOTIFY,
                                       CMD_TYPE_NOTIFICATION, -100, -100, 0, 0);
                continue;
            }

            g_track_state.track_redetect_ = false;
            g_track_state.track_lost_ = false;
            lost_num = 0;

            rkfree(frame_mb_copy);

            // time2.start();
            int out_x = g_track_result_filter[0]->filter(result.bbox.x);
            int out_y = g_track_result_filter[1]->filter(result.bbox.y);
            int out_w = g_track_result_filter[2]->filter(result.bbox.width);
            int out_h = g_track_result_filter[3]->filter(result.bbox.height);
            float out_center_pox_x =
                g_track_result_filter[4]->filter(result.center_pos.x);
            float out_center_pox_y =
                g_track_result_filter[5]->filter(result.center_pos.y);
            // time2.stop();
            // time2.show_distance("Update stage ---- filter cost time");

            wsResNotifyTrackResult(CMD_NOTIFY_TRACK_RESULT, MODULE_NOTIFY,
                                   CMD_TYPE_NOTIFICATION, out_x, out_y, out_w, out_h);

            time1.stop();
            int time_process = time1.distance();
            g_track_result.center_pos =
                cv::Point2d(out_center_pox_x, out_center_pox_y);
            g_track_result.cost_time = time_process;
            g_track_result.bscore = result.bscore;

            std::unique_lock<std::mutex> locker(g_track_pid_mutex);
            g_track_state.track_param_update_ = true;
            locker.unlock();

            box_center_pos.x = out_center_pox_x;
            box_center_pos.y = out_center_pox_y;
            std::ofstream box_center_pos_x_file("box_center_pos_x.txt", std::ios::out | std::ios::app);
            box_center_pos_x_file << box_center_pos.x << std::endl;
            box_center_pos_x_file.close();

            std::ofstream box_center_pos_y_file("box_center_pos_y.txt", std::ios::out | std::ios::app);
            box_center_pos_y_file << box_center_pos.y << std::endl;
            box_center_pos_y_file.close();
        }
        else
        {
            log_e("get frame_mb failed");
            break;
        }
    }

    g_track_state.track_finish_ = true;
    if (b_tracking == false)
        wsCommonResponse(CMD_TRACK_STOP_TRACK, MODULE_TRACK, CMD_TYPE_RESPONSE,
                         WS_OK);
    else
        wsCommonResponse(CMD_TRACK_START_TRACK, MODULE_TRACK, CMD_TYPE_RESPONSE,
                         -CODE_TRACK_TRACKER_FAILED);

    for (int i = 0; i < g_track_result_filter.size(); ++i)
    {
        delete g_track_result_filter[i];
    }
    g_track_result_filter.clear();

    setAndNotifyTrackState(STATE_STOPPED);
    setAndNotifyTrackState(STATE_IDLE);

    rgb_power_.setRgbEffect(RGB_EFFECT_SUCCESS);

    return 0;
}

int Track::startTrackSentryMode(const WsPacket &ws_packet)
{
    log_i("startTrackSentryMode start -----------------------------");
    sentry_mode_ = true;
    ReqStartSentryMode req_start_track;
    string serialized_data = ws_packet.data();

    if (!req_start_track.ParseFromString(serialized_data))
    {
        wsCommonResponse(CMD_SENTRY_MODE_START, MODULE_TRACK, CMD_TYPE_RESPONSE,
                         -WS_PARSE_PROTOBUF_ERROR);
        return -WS_PARSE_PROTOBUF_ERROR;
    }

    int sm_class = req_start_track.mode();

    if (b_tracker_init == false)
    {
        wsCommonResponse(CMD_SENTRY_MODE_START, MODULE_TRACK, CMD_TYPE_RESPONSE,
                         -CODE_TRACK_SENTRY_MODE_INITING);
        return -CODE_TRACK_SENTRY_MODE_INITING;
    }

    if (sentry_mode_state != STATE_SENTRY_MODE_IDLE)
    {
        stopTrack();
        while (sentry_mode_state != STATE_SENTRY_MODE_IDLE)
            usleep(50 * 1000);
    }

    setAndNotifySentryModeState(STATE_SENTRY_MODE_INIT);
    rgb_power_.setRgbEffect(RGB_EFFECT_WORKING_STATE);

    cv::Size cam_tele_frame_size(1280, 720);
    int box_max_width = std::round(cam_tele_frame_size.width / 12.2327) * 0.8;
    int box_max_height = std::round(cam_tele_frame_size.height / 12.5140) * 0.8;
    b_tracking = true;
    MotorController mc;

    // ���¼��㳤������͹�ǻ����ƥ���
    focus_.calcCurTemplateRect();

    // ��λ�����ȡ�ڱ�ģʽ��ʼ�ľ���λ��
    double yaw_absolute_angle, pitch_absolute_angle;
    int ret =
        initSentryModeMotorPosition(yaw_absolute_angle, pitch_absolute_angle);
    if (ret < 0)
    {
        setAndNotifySentryModeState(STATE_SENTRY_MODE_IDLE);
        wsCommonResponse(CMD_SENTRY_MODE_START, MODULE_TRACK, CMD_TYPE_RESPONSE,
                         -CODE_TRACK_SENTRY_MODE_FAILED);
        return -CODE_TRACK_SENTRY_MODE_FAILED;
    }

    log_i("start track sentry mode. sm class : %d", sm_class);
    time_checker time1;
    setAndNotifySentryModeState(STATE_SENTRY_MODE_DETECT);
    while (b_tracking)
    {
        bool detect_success = false;
        bool doDualCameraLinkage = false;
        MEDIA_BUFFER tele_frame_mb_copy;
        cv::Rect init_rect;
        ret = -1;

        // ��Ǽ��
        detect_result_group_t detect_result_group_list[2];
        ret = wideDetect(detect_result_group_list);
        if (ret == 0)
        {
            for (auto detect_result_group : detect_result_group_list)
            {
                for (int i = 0; i < detect_result_group.count; ++i)
                {
                    // ���ݹ�Ǻͳ�������ı���������Ŀ�곤����ͷ�Ƿ���cover
                    cv::Point det_box_center_pos;
                    ret = checkWideDetectResult(detect_result_group.results[i],
                                                box_max_width, box_max_height,
                                                det_box_center_pos);
                    if (ret < 0)
                    {
                        continue;
                    }
#ifdef DEBUG_TRACK
                    time1.start();
#endif
                    // ��ʼ˫�������ƶ��������浽��⵽��Ŀ��λ��
                    mc.startDualCameraLinkage(det_box_center_pos);

                    // ˫���������涶�����أ���Ҫ��ʱ400ms
                    std::this_thread::sleep_for(std::chrono::milliseconds(400));
                    doDualCameraLinkage = true;
#ifdef DEBUG_TRACK
                    time1.stop();
                    time1.show_distance(
                        "-------------------  detect startDualCameraLinkage cost : ");
#endif
                    // ������һ��ȷ�ϻ������Ƿ���Ŀ�꣬�Լ�����Ŀ���box
                    teleDetect(cam_tele_frame_size.width, cam_tele_frame_size.height,
                               tele_frame_mb_copy, detect_success, init_rect);

                    // �������˫����������δ��⵽Ŀ�꣬�����»ص���ʼλ�ã�Ѱ������Ŀ��
                    if (!detect_success)
                    {
                        resetSentryModelPosition(yaw_absolute_angle, pitch_absolute_angle);
                    }
                    else
                    {
                        break;
                    }
                }

                if (doDualCameraLinkage)
                {
                    break;
                }
            }
        }

        // ������û�м�⵽��ʹ�ó�����⣬����ι��������
        if (!detect_success)
        {
            teleDetect(cam_tele_frame_size.width, cam_tele_frame_size.height,
                       tele_frame_mb_copy, detect_success, init_rect);
        }

        // ���������⵽Ŀ��
        if (detect_success)
        {
            setAndNotifySentryModeState(STATE_SENTRY_MODE_TRACK);
            log_i("Sentry Mode start record and track. init_rect : %d %d %d %d",
                  init_rect.x, init_rect.y, init_rect.width, init_rect.height);
            // ��ʼ¼���ҽ��и���, ������ʱ����pid������С�ƶ��ٶ�Ϊ300
            ret = cam_tele_.startRecord(false);
            if (ret == 0)
            {
                int last_g_pid_min_speed = g_pid_min_speed;
                g_pid_min_speed = 300;
                trackWithImage(tele_frame_mb_copy, init_rect);
                g_pid_min_speed = last_g_pid_min_speed;
                rkfree(tele_frame_mb_copy);
                cam_tele_.stopRecord(false);
                setAndNotifySentryModeState(STATE_SENTRY_MODE_TRACK_FINISH);
            }
            else
            {
                rkfree(tele_frame_mb_copy);
                // ��ʼ¼��ʧ�ܣ��ͷ�����Ϣ
                wsCommonResponse(CMD_SENTRY_MODE_START, MODULE_TRACK, CMD_TYPE_RESPONSE, ret);
                break;
            }

            // ������󣬻ص��ڱ�ģʽ��ʼ��
            resetSentryModelPosition(yaw_absolute_angle, pitch_absolute_angle);

            setAndNotifySentryModeState(STATE_SENTRY_MODE_DETECT);
        }
        std::this_thread::sleep_for(std::chrono::microseconds(10));
        // break;
    }

    if (b_tracking == false)
    {
        wsCommonResponse(CMD_SENTRY_MODE_STOP, MODULE_TRACK, CMD_TYPE_RESPONSE,
                         WS_OK);
    }

    setAndNotifySentryModeState(STATE_SENTRY_MODE_IDLE);

    rgb_power_.setRgbEffect(RGB_EFFECT_SUCCESS);

    return 0;
}

int Track::stopTrack()
{
    log_i("stop track, sentry_mode_ : %d", sentry_mode_);
    if (sentry_mode_)
    {
        if (sentry_mode_state != STATE_SENTRY_MODE_IDLE && sentry_mode_state != STATE_SENTRY_MODE_STOPPING)
        {
            setAndNotifySentryModeState(STATE_SENTRY_MODE_STOPPING);
        }
        else if (sentry_mode_state == STATE_SENTRY_MODE_IDLE)
        {
            setAndNotifySentryModeState(STATE_SENTRY_MODE_IDLE);
        }
    }
    else
    {
        if (tracker_state != STATE_IDLE && tracker_state != STATE_STOPPING)
            setAndNotifyTrackState(STATE_STOPPING);
        else if (tracker_state == STATE_IDLE)
            setAndNotifyTrackState(STATE_IDLE);
    }
    b_tracking = false;

    return 0;
}

int Track::stopTrack(const WsPacket &ws_packet)
{
    // lock_guard<mutex> lock(track_mutex);
    int ret = stopTrack();
    return ret;
}

int Track::trackWithImage(MEDIA_BUFFER init_frame_mb, cv::Rect init_rect)
{
    cv::Size frame_size(1280, 720);

    uint64_t frame_mb_id = 0;
    MEDIA_BUFFER frame_mb_copy = NULL;
    MEDIA_BUFFER frame_mb = NULL;

    int x = init_rect.x;
    int y = init_rect.y;
    int w = init_rect.width;
    int h = init_rect.height;

    Rect2d box = Rect2d(x, y, w, h);
    Point2d center_pos =
        Point2d((frame_size.width - 1) / 2, (frame_size.height - 1) / 2);

    time_checker time1, time2;
    time1.start();
    log_i("start init tracker");

    int ret = siam_rpn_tracker->init(init_frame_mb, frame_size, box);
    if (!ret)
    {
        log_e("preprocess failed");
        return -CODE_TRACK_TRACKER_FAILED;
    }
    time1.stop();

    Point2d box_center_pos =
        box.tl() + Point2d((box.width - 1) / 2, (box.height - 1) / 2);

    if (g_pid_thread.joinable())
        g_pid_thread.join();
    g_pid_thread = std::thread(&pid_thread, this);

    int lost_num = 0;
    // b_tracking = true;
    g_track_result.center_pos = center_pos;
    g_track_state.track_finish_ = false;
    g_track_state.track_param_update_ = false;
    g_track_state.track_lost_ = false;
    g_track_state.track_object_moving_ = true;
    g_track_state.track_first_start = true;
    g_track_state.track_redetect_ = false;

    auto filter_x = new OneEuroFilter(30, 0.5, 0.1, 30.0);
    auto filter_y = new OneEuroFilter(30, 0.5, 0.1, 30.0);
    auto filter_w = new OneEuroFilter(30, 0.5, 0.1, 30.0);
    auto filter_h = new OneEuroFilter(30, 0.5, 0.1, 30.0);
    auto filter_center_pos_x = new OneEuroFilter(30, 0.5, 0.1, 30.0);
    auto filter_center_pos_y = new OneEuroFilter(30, 0.5, 0.1, 30.0);
    g_track_result_filter.push_back(filter_x);
    g_track_result_filter.push_back(filter_y);
    g_track_result_filter.push_back(filter_w);
    g_track_result_filter.push_back(filter_h);
    g_track_result_filter.push_back(filter_center_pos_x);
    g_track_result_filter.push_back(filter_center_pos_y);

    int track_frame_count = 0;
    while (b_tracking)
    {
        time1.start();
        // frame_mb = cam_tele_.getNv12MediaBuffer();
        uint64_t cur_frame_mb_id;
        cam_tele_.getNv12MediaBufferWithID(&frame_mb, &cur_frame_mb_id);

        if (frame_mb_id - cur_frame_mb_id == 0)
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(3));
            if (frame_mb != NULL)
            {
                rkfree(frame_mb);
            }
            continue;
        }

        frame_mb_id = cur_frame_mb_id;

        track_frame_count++;
        if (track_frame_count > 10)
        {
            g_track_state.track_first_start = false;
        }

        if (frame_mb != NULL)
        {
            frame_mb_copy = RK_MPI_MB_Copy(frame_mb, RK_TRUE);
            // time2.start();
            int ret = siam_rpn_tracker->preprocess_img(frame_mb, frame_size,
                                                       box_center_pos); // preprocess
            rkfree(frame_mb);
            if (ret < 0)
            {
                log_e("preprocess failed");
                rkfree(frame_mb_copy);
                break;
            }
            track_result result =
                siam_rpn_tracker->track(frame_size, g_track_state.track_redetect_);
            // time2.stop();
            // time2.show_distance("Update stage ---- track cost time");
#ifdef DEBUG_TRACK
            log_i(
                "######## result.bscore : %f, lost_num : %d, "
                "g_track_state.track_lost_ : %d, g_track_state.track_redetect_ : %d",
                result.bscore, lost_num, g_track_state.track_lost_,
                g_track_state.track_redetect_);
#endif
            if (result.bscore < 0.1)
            {
                lost_num++;
                if (lost_num >= 10)
                {
                    g_track_state.track_lost_ = true;
                    g_track_state.track_redetect_ = true;
                }
                else if (lost_num >= 1)
                {
                    g_track_state.track_lost_ = true;
                }
            }

            if (g_track_state.track_redetect_)
            {
                if (result.bscore > 0.98)
                {
                    lost_num = 0;
                    g_track_state.track_redetect_ = false;
                    g_track_state.track_lost_ = false;
                }
            }

            if (lost_num > 60)
            {
                lost_num = 0;
                rkfree(frame_mb_copy);
                break;
            }

            if (g_track_state.track_redetect_ || (g_track_state.track_lost_ && result.bscore < 0.6))
            {
                if (result.bscore >= 0.1)
                {
                    lost_num++;
                }
                rkfree(frame_mb_copy);
                wsResNotifyTrackResult(CMD_NOTIFY_SENTRY_MODE_TRACK_RESULT,
                                       MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, -100, -100,
                                       0, 0);
                continue;
            }

            g_track_state.track_redetect_ = false;
            g_track_state.track_lost_ = false;
            lost_num = 0;

            rkfree(frame_mb_copy);

            // time2.start();
            int out_x = g_track_result_filter[0]->filter(result.bbox.x);
            int out_y = g_track_result_filter[1]->filter(result.bbox.y);
            int out_w = g_track_result_filter[2]->filter(result.bbox.width);
            int out_h = g_track_result_filter[3]->filter(result.bbox.height);
            float out_center_pox_x =
                g_track_result_filter[4]->filter(result.center_pos.x);
            float out_center_pox_y =
                g_track_result_filter[5]->filter(result.center_pos.y);
            // time2.stop();
            // time2.show_distance("Update stage ---- filter cost time");

            wsResNotifyTrackResult(CMD_NOTIFY_SENTRY_MODE_TRACK_RESULT, MODULE_NOTIFY,
                                   CMD_TYPE_NOTIFICATION, out_x, out_y, out_w, out_h);

            time1.stop();
            int time_process = time1.distance();
            g_track_result.center_pos =
                cv::Point2d(out_center_pox_x, out_center_pox_y);
            g_track_result.cost_time = time_process;
            g_track_result.bscore = result.bscore;

            std::unique_lock<std::mutex> locker(g_track_pid_mutex);
            g_track_state.track_param_update_ = true;
            locker.unlock();

            box_center_pos.x = out_center_pox_x;
            box_center_pos.y = out_center_pox_y;
            // time1.show_distance("Update stage ---- total cost time");
        }
        else
        {
            log_e("get frame_mb failed");
            break;
        }
    }

    wsResNotifyTrackResult(CMD_NOTIFY_SENTRY_MODE_TRACK_RESULT, MODULE_NOTIFY,
                           CMD_TYPE_NOTIFICATION, -100, -100, 0, 0);
    g_track_state.track_finish_ = true;

    for (int i = 0; i < g_track_result_filter.size(); ++i)
    {
        delete g_track_result_filter[i];
    }
    g_track_result_filter.clear();

    return 0;
}

int Track::wideDetect(detect_result_group_t *detect_result_group)
{
#ifdef DEBUG_TRACK
    time_checker time1;
    time1.start();
#endif
    cv::Mat wide_img;
    cam_wide_.getJpgDecodeMat(wide_img);
    if (wide_img.empty())
    {
        return -1;
    }
#ifdef DEBUG_TRACK
    time1.stop();
    time1.show_distance("-------------------  detect getJpgDecodeMat cost : ");
#endif
    // Ŀǰ�����1280x720��ͼ���Ȳ��Ϊ����720x720��ͼ�񣬱���ͼ������̫��
    cv::Mat wide_img1 = wide_img(cv::Rect(0, 0, 720, 720)).clone();
    cv::Mat wide_img2 = wide_img(cv::Rect(560, 0, 720, 720)).clone();
    std::vector<cv::Mat> detect_img_list;
    detect_img_list.push_back(wide_img1);
    detect_img_list.push_back(wide_img2);

    for (int k = 0; k < detect_img_list.size(); ++k)
    {
        cv::Mat cur_detect_img = detect_img_list[k];
#ifdef DEBUG_TRACK
        time1.start();
#endif
        g_yolov8->predict(cur_detect_img, detect_result_group[k]);
#ifdef DEBUG_TRACK
        time1.stop();
        time1.show_distance("-------------------  detect yolov8 predict cost : ");

        // printf("DRAWING OBJECT\n");
        // cv::Mat frame = cur_detect_img.clone();
        // drawObject(frame, detect_result_group[k],
        //            "test_yolov8_" + std::to_string(k) + ".jpg");
#endif
        // ת�����ͼ�������
        if (k == 1)
        {
            for (int i = 0; i < detect_result_group[k].count; i++)
            {
                detect_result_group[k].results[i].box.left += 560;
                detect_result_group[k].results[i].box.right += 560;
            }
        }
#ifdef DEBUG_TRACK
        log_i("yolov8 detect finish. detect_result_group count : %d",
              detect_result_group[k].count);
#endif
    }

    return 0;
}

int Track::teleDetect(int cam_tele_width, int cam_tele_height,
                      MEDIA_BUFFER &tele_frame_mb_copy, bool &detect_success,
                      cv::Rect &init_rect)
{
    MEDIA_BUFFER tele_image_nv12 = cam_tele_.getNv12MediaBuffer();
    if (!tele_image_nv12)
    {
        return -1;
    }

    detect_result_group_t tele_detect_result_group;
#ifdef DEBUG_TRACK
    time_checker time1;
    time1.start();
#endif
    g_yolov8->predict_with_nv12(tele_image_nv12,
                                cv::Size(cam_tele_width, cam_tele_height),
                                tele_detect_result_group);
#ifdef DEBUG_TRACK
    time1.stop();
    time1.show_distance(
        "-------------------  detect tele yolov8 predict cost : ");

    // // MEDIA_BUFFER dst_mb = cam_tele_.getRgbMB_720p(cam_tele_width,
    // // cam_tele_height);
    // cv::Mat tele_image1(cam_tele_height * 1.5, cam_tele_width, CV_8UC1,
    //                     (void *)RK_MPI_MB_GetPtr(tele_image_nv12));
    // cv::Mat tele_image;
    // cv::cvtColor(tele_image1, tele_image, cv::COLOR_YUV2BGR_NV12);
    // drawObject(tele_image, tele_detect_result_group, "tele_detect.jpg");

#endif
    if (tele_detect_result_group.count > 0)
    {
#ifdef DEBUG_TRACK
        for (int i = 0; i < tele_detect_result_group.count; i++)
        {
            detect_result_t *tele_det_result = &(tele_detect_result_group.results[i]);
            printf("%s%s @ (%d %d %d %d) %f\n", "---------------",
                   tele_det_result->name, tele_det_result->box.left,
                   tele_det_result->box.top, tele_det_result->box.right,
                   tele_det_result->box.bottom, tele_det_result->prop);
        }
#endif
        for (int i = 0; i < tele_detect_result_group.count; ++i)
        {
            detect_result_t tele_det_result = tele_detect_result_group.results[i];
            // ��ֵ̫�Ͳ����и���
            if (tele_det_result.prop > 0.7)
            {
                BOX_RECT tele_det_box = tele_det_result.box;
                int c_x = (tele_det_box.left + tele_det_box.right) / 2;
                int c_y = (tele_det_box.top + tele_det_box.bottom) / 2;

                init_rect.width = tele_det_box.right - tele_det_box.left + 1;
                init_rect.height = tele_det_box.bottom - tele_det_box.top + 1;

                // �������������������С����
                if (init_rect.width < 60 && init_rect.height < 60)
                {
                    continue;
                }
                if (init_rect.width > cam_tele_width / 2)
                {
                    init_rect.width = cam_tele_width / 2;
                }
                if (init_rect.height > cam_tele_height / 2)
                {
                    init_rect.height = cam_tele_height / 2;
                }

                init_rect.x = c_x - init_rect.width / 2;
                init_rect.y = c_y - init_rect.height / 2;

                tele_frame_mb_copy = RK_MPI_MB_Copy(tele_image_nv12, RK_TRUE);
                rkfree(tele_image_nv12);
                detect_success = true;
                break;
            }
        }
    }
    rkfree(tele_image_nv12);

    return 0;
}

int Track::checkWideDetectResult(detect_result_t det_result, int box_max_width,
                                 int box_max_height,
                                 cv::Point &det_box_center_pos)
{
    // ��ֵ̫�Ͳ����и���
    if (det_result.prop < 0.5)
    {
        return -1;
    }

    BOX_RECT det_box = det_result.box;
    // Ŀ���С̫�󲻽��и���
    int det_box_width = det_box.right - det_box.left;
    int det_box_height = det_box.bottom - det_box.top;
    // log_i("det_box left : %d, right : %d, top : %d, bottom : %d, width : %d,
    // height : %d",
    //         det_box.left, det_box.right, det_box.top, det_box.bottom,
    //         det_box_width, det_box_height);
    if (det_box_width > box_max_width && det_box_height > box_max_height)
    {
        return -2;
    }

    // �����������Ŀ�곤����ͷ����cover
    if (det_box_width < box_max_width && det_box_height < box_max_height)
    {
        det_box_center_pos = cv::Point((det_box.left + det_box.right) * 0.5,
                                       (det_box.bottom + det_box.top) * 0.5);
    }
    else if (det_box_width < box_max_width)
    {
        det_box_center_pos = cv::Point((det_box.left + det_box.right) * 0.5,
                                       det_box.top + box_max_height * 0.5);
    }
    else if (det_box_height < box_max_height)
    {
        det_box_center_pos = cv::Point(det_box.left + box_max_width * 0.5,
                                       (det_box.bottom + det_box.top) * 0.5);
    }
    else
    {
        return -3;
    }

    return 0;
}

int Track::initSentryModeMotorPosition(double &yaw_position,
                                       double &pitch_position)
{
    // �����λ
    int ret = motor_yaw.resetAndSetParameterInDegree();
    if (ret < 0)
    {
        return -1;
    }

    motor_yaw.run();

    ret = motor_pitch.resetAndSetParameterInDegree();
    if (ret < 0)
    {
        return -2;
    }

    motor_pitch.run();

    motor_yaw.waitMotorToStop();
    motor_pitch.waitMotorToStop();

    motor_yaw.getAbsolutePositionInDegree(STEP_MOTOR_MINISTEP_8, yaw_position);
    motor_pitch.getAbsolutePositionInDegree(STEP_MOTOR_MINISTEP_8,
                                            pitch_position);

    log_i(
        "initSentryModeMotorPosition, yaw_absolute_angle : %f, "
        "pitch_absolute_angle : %f",
        yaw_position, pitch_position);
    return 0;
}

int Track::resetSentryModelPosition(double yaw_absolute_angle,
                                    double pitch_absolute_angle)
{
    log_i(
        "resetSentryModelPosition, yaw_absolute_angle : %f, pitch_absolute_angle "
        ": %f",
        yaw_absolute_angle, pitch_absolute_angle);
    motor_yaw.setParameterInPositionInDegree(
        yaw_absolute_angle, 10, STEP_MOTOR_MINISTEP_8, STEP_MOTOR_RAMP_PULSE_0);
    motor_pitch.setParameterInPositionInDegree(
        pitch_absolute_angle, 10, STEP_MOTOR_MINISTEP_8, STEP_MOTOR_RAMP_PULSE_0);

    motor_yaw.run();
    motor_pitch.run();

    motor_yaw.waitMotorToStop();
    motor_pitch.waitMotorToStop();

    return 0;
}

// ����ʽ���
void Track::polyFit(const std::vector<cv::Point2f> &points, const int order,
                    cv::Mat *coeff)
{
    const int n = points.size();
    cv::Mat A = cv::Mat::ones(n, order + 1, CV_64FC1);
    cv::Mat B = cv::Mat::zeros(n, 1, CV_64FC1);

    for (int i = 0; i < n; ++i)
    {
        const double a = points.at(i).x;
        const double b = points.at(i).y;
        B.at<double>(i, 0) = b;
        if (i > 0)
        {
            for (int j = 1, v = a; j < order + 1; ++j, v *= a)
            {
                A.at<double>(i, j) = v;
            }
        }
    }

    (*coeff) = (A.t() * A).inv() * A.t() * B;
}

float Track::pidKpRatio(const std::vector<float> &arr, int width,
                        float k_thre)
{
    float kp_ratio = 1.0;
    size_t arr_size = arr.size();
    if (arr_size < 10)
    {
        return kp_ratio;
    }

    std::vector<cv::Point2f> raw_points;

    for (size_t i = 0; i < arr_size; ++i)
    {
        raw_points.push_back(cv::Point2f(i, arr[i]));
    }

    cv::Mat coeff;
    polyFit(raw_points, 1, &coeff);
    float k = coeff.at<double>(1, 0);
    float b = coeff.at<double>(0, 0);
    float y1 = k * (arr_size - 1) + b;
    float y2 = k * (arr_size - 2) + b;

    if (abs(y1) > abs(y2) && k * b > 0)
    {
        if (abs(k) > k_thre && abs(y1) > width / 3.2f)
        {
            kp_ratio = 1.2f;
        }
        else if (abs(k) > k_thre / 2.0f && abs(y1) > width / 5.0f)
        {
            kp_ratio = 1.1f;
        }
    }

    // log_i("####### k : %f, b : %f, coeff : %f, kp_ratio : %f",k ,b,
    // coeff.at<double>(1, 0), kp_ratio);
    return kp_ratio;
}

int Track::convertSpeed2Pixel(float fov, int freq, float time,
                              int total_pixel_length)
{
    MotorController mc;
    float step_angle = 1.8f;
    float delta_degree =
        step_angle * (freq / mc.motor_yaw_gear_ratio_) * time * 0.001f;
    int delta_pixel = delta_degree / fov * total_pixel_length;
    return delta_pixel;
}

#ifdef DEBUG_TRACK
void replaceString(std::string &str, const std::string &oldStr,
                   const std::string &newStr)
{
    size_t pos = str.find(oldStr);
    while (pos != std::string::npos)
    {
        str.replace(pos, oldStr.length(), newStr);
        pos = str.find(oldStr, pos + newStr.length());
    }
}

void drawObject(cv::Mat image, detect_result_group_t detect_result_group,
                std::string save_file)
{
    for (int i = 0; i < detect_result_group.count; i++)
    {
        detect_result_t *det_result = &(detect_result_group.results[i]);
        printf("%s%s @ (%d %d %d %d) %f\n", "---------------", det_result->name,
               det_result->box.left, det_result->box.top, det_result->box.right,
               det_result->box.bottom, det_result->prop);
        int x1 = det_result->box.left;
        int y1 = det_result->box.top;
        int x2 = det_result->box.right;
        int y2 = det_result->box.bottom;
        // draw box

        cv::rectangle(image, cv::Point(x1, y1), cv::Point(x2, y2),
                      cv::Scalar(0, 255, 0), 2);
        cv::putText(image, det_result->name, cv::Point(x1, y1 - 24),
                    cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(0, 0, 255), 2);
        cv::putText(image, std::to_string(det_result->prop), cv::Point(x1, y1 + 12),
                    cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(0, 0, 255), 2);
    }
    cv::imwrite(save_file, image);
}
#endif

#ifdef DEBUG_TRACK
int matchTemplateInTele(const cv::Mat &src_img, const cv::Mat &temp_img,
                        cv::Rect &out_rect)
{
    // ����ģ��ƥ��
    cv::Mat resultImage;
    cv::matchTemplate(src_img, temp_img, resultImage, cv::TM_CCOEFF_NORMED);

    // ��ȡƥ����
    double minVal, maxVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc(resultImage, &minVal, &maxVal, &minLoc, &maxLoc);

    double threshold = 0.2;
    printf("matchTemplateInTele maxVal : %f\n", maxVal);
    if (maxVal > threshold)
    {
        out_rect = cv::Rect(maxLoc.x, maxLoc.y, temp_img.cols, temp_img.rows);
    }
    else
    {
        return -1;
    }

    return 0;
}
#endif
